﻿using System;
using System.Collections.Generic;
using System.Text;
using KukaDataCom;
using KukaDataComCommon;

namespace CubeCalculations
{
   public class kr3_calc
   {

      static float A1Up = 0f, A1Front = -0.15f, A6Flip = -135f, A6Start = 45.5f;

      public static
         Dictionary<Positions, AxisPosition> AxisPositions = new Dictionary<Positions, AxisPosition>
                                                                {
                                                                   {
                                                                      Positions.StartPositionUp, new AxisPosition
                                                                                                    {
                                                                                                       A1 = A1Up,
                                                                                                       //- 0.07382812f,
                                                                                                       A2 = -34.74492f,
                                                                                                       A3 = 72.1125f,
                                                                                                       A4 = 0f,
                                                                                                       A5 = 50.55249f,
                                                                                                       A6 = A6Start
                                                                                                       //43.25098f
                                                                                                    }
                                                                      },
                                                                   {
                                                                      Positions.StartPositionDown, new AxisPosition
                                                                                                      {
                                                                                                         A1 = A1Up,
                                                                                                         // -0.07382812f,
                                                                                                         A2 = -29.13047f,
                                                                                                         A3 = 71.06309f,
                                                                                                         A4 = 0f,
                                                                                                         A5 = 45.98438f,
                                                                                                         A6 = A6Start
                                                                                                         //43.00049f
                                                                                                      }
                                                                      },
                                                                   {
                                                                      Positions.StartPositionDownSwingLeft,
                                                                      new AxisPosition
                                                                         {
                                                                            A1 = A1Up,
                                                                            // -0.07382812f,
                                                                            A2 = -29.13047f,
                                                                            A3 = 71.06309f,
                                                                            A4 = 0f,
                                                                            A5 = 45.98438f,
                                                                            A6 = 45.5f
                                                                         }
                                                                      },
                                                                   {
                                                                      Positions.StartPositionDownSwingRight,
                                                                      new AxisPosition
                                                                         {
                                                                            A1 = A1Up,
                                                                            // -0.07382812f,
                                                                            A2 = -29.13047f,
                                                                            A3 = 71.06309f,
                                                                            A4 = 0f,
                                                                            A5 = 45.98438f,
                                                                            A6 = 43.25098f
                                                                         }
                                                                      },
                                                                   {
                                                                      Positions.FrontClockwiseDown, new AxisPosition
                                                                                                       {
                                                                                                          A1 = A1Up,
                                                                                                          //-0.07382812f,
                                                                                                          A2 = -28.984570f,
                                                                                                          A3 = 70.748440f,
                                                                                                          A4 = 0f,
                                                                                                          A5 = 46.15356f,
                                                                                                          A6 = -45f
                                                                                                       }
                                                                      },
                                                                   {
                                                                      Positions.FrontClockwiseDownSwing, new AxisPosition
                                                                                                       {
                                                                                                          A1 = A1Up,
                                                                                                          //-0.07382812f,
                                                                                                          A2 = -28.984570f,
                                                                                                          A3 = 70.748440f,
                                                                                                          A4 = 0f,
                                                                                                          A5 = 46.15356f,
                                                                                                          A6 = -47f
                                                                                                       }
                                                                      },
                                                                   {
                                                                      Positions.FrontClockwiseUp, new AxisPosition
                                                                                                     {
                                                                                                        A1 = A1Up,
                                                                                                        // -0.07382812f,
                                                                                                        A2 = -34.74492f,
                                                                                                        A3 = 72.1125f,
                                                                                                        A4 = 0f,
                                                                                                        A5 = 50.55249f,
                                                                                                        A6 = -46.99951f
                                                                                                     }
                                                                      },
                                                                   {
                                                                      Positions.FrontCounterClockwiseDown,
                                                                      new AxisPosition
                                                                         {
                                                                            A1 = A1Up,
                                                                            //-0.07382812f,
                                                                            A2 = -29.13047f,
                                                                            A3 = 71.06309f,
                                                                            A4 = 0f,
                                                                            A5 = 45.98438f,
                                                                            A6 = 135.501f
                                                                         }
                                                                      },
                                                                   {
                                                                      Positions.FrontCounterClockwiseUp,
                                                                      new AxisPosition
                                                                         {
                                                                            A1 = A1Up,
                                                                            //-0.07382812f,
                                                                            A2 = -34.74492f,
                                                                            A3 = 72.1125f,
                                                                            A4 = 0f,
                                                                            A5 = 50.55249f,
                                                                            A6 = 135.501f
                                                                         }
                                                                      },
                                                                   {
                                                                      Positions.FlipTopUp, new AxisPosition
                                                                                              {
                                                                                                 A1 = A1Up,
                                                                                                 //-0.07382812f,
                                                                                                 A2 = -32.31738f,
                                                                                                 A3 = 66.81445f,
                                                                                                 A4 = 0f,
                                                                                                 A5 = 53.41333f,
                                                                                                 A6 = A6Flip //- 136.50f
                                                                                              }
                                                                      },
                                                                   {
                                                                      Positions.FlipTopUpToDown, new AxisPosition
                                                                                                    {
                                                                                                       A1 = A1Up,
                                                                                                       A2 = -26.86992f,
                                                                                                       A3 = 63.40781f,
                                                                                                       A4 = 0f,
                                                                                                       A5 = 51.36987f,
                                                                                                       A6 = A6Flip
                                                                                                       
                                                                                                       //A2 = -26.12f,
                                                                                                       //A3 = 63.21f,
                                                                                                       //A4 = 0f,
                                                                                                       //A5 = 50.82f,
                                                                                                       //A6 = A6Flip
                                                                                                    }
                                                                      },
                                                                   {
                                                                      Positions.FlipTopDown, new AxisPosition
                                                                                                {
                                                                                                   //A1 = sA1 ,// -0.07382812f,
                                                                                                   //A2 = -26.41289f,
                                                                                                   //A3 = 65.64551f,
                                                                                                   //A4 = 0f,
                                                                                                   //A5 = 48.67603f,
                                                                                                   //A6 = -136.50f
                                                                                                   A1 = A1Up,
                                                                                                   // -0.07382812f,
                                                                                                   A2 = -26.58164f,
                                                                                                   A3 = 66.0041f,
                                                                                                   A4 = 0f,
                                                                                                   A5 = 48.48706f,
                                                                                                   A6 = A6Flip
                                                                                                   //-136.50f
                                                                                                }
                                                                      },
                                                                   {
                                                                      Positions.FlipTopP1, new AxisPosition
                                                                                              {
                                                                                                 A1 = A1Up,
                                                                                                 //-0.07382812f,
                                                                                                 A2 = -36.41836f,
                                                                                                 A3 = 66.67031f,
                                                                                                 A4 = 0f,
                                                                                                 A5 = 57.65625f,
                                                                                                 A6 = A6Flip //-136.50f
                                                                                              }
                                                                      },
                                                                   {
                                                                      Positions.FlipTopP2, new AxisPosition
                                                                                              {
                                                                                                 A1 = A1Up,
                                                                                                 // -0.07382812f,
                                                                                                 A2 = -34.74492f,
                                                                                                 A3 = 72.1125f,
                                                                                                 A4 = 0f,
                                                                                                 A5 = 0f,
                                                                                                 A6 = A6Flip //-136.50f
                                                                                              }
                                                                      },
                                                                   {
                                                                      Positions.FlipFrontUp, new AxisPosition
                                                                                                {
                                                                                                   A1 = A1Front,
                                                                                                   // -0.07382812f,
                                                                                                   A2 = -26.7426f,
                                                                                                   A3 = 121.5967f,
                                                                                                   A4 = 0f,
                                                                                                   A5 = -96.15234f,
                                                                                                   A6 = A6Flip
                                                                                                   //-136.50f
                                                                                                }
                                                                      },
                                                                   {
                                                                      Positions.FlipFrontUpToDown, new AxisPosition
                                                                                                      {
                                                                                                         A1 = A1Front,
                                                                                                         // -0.07382812f,
                                                                                                         A2 = -3.5f,
                                                                                                         A3 = 101.25f,
                                                                                                         A4 = 0f,
                                                                                                         A5 = -99.75f,
                                                                                                         A6 = A6Flip
                                                                                                         //-136.50f
                                                                                                      }
                                                                      },
                                                                   {
                                                                      Positions.FlipFrontDown, new AxisPosition
                                                                                                  {
                                                                                                     //A1 =  sA1 ,//-0.07382812f,
                                                                                                     //A2 = -1.645313f,
                                                                                                     //A3 = 102.5525f,
                                                                                                     //A4 = 0f,
                                                                                                     //A5 = -102.208f,
                                                                                                     //A6 = -136.50f
                                                                                                     A1 = A1Front,
                                                                                                     //-0.07382812f,
                                                                                                     A2 = -1.51875f,
                                                                                                     A3 = 102.4277f,
                                                                                                     A4 = 0f,
                                                                                                     A5 = -102.2102f,
                                                                                                     A6 = A6Flip
                                                                                                     //-136.50f
                                                                                                  }
                                                                      },
                                                                   {
                                                                      Positions.CameraFront, new AxisPosition
                                                                                                  {
                                                                                                  //   A1 = 21.63691f,
                                                                                                  //   A2=-26.6168f,
                                                                                                  //   A3 = 55.77891f,
                                                                                                  //   A4 = 0,
                                                                                                  //   A5=58.90649f,
                                                                                                  //   A6 = 67.59220f
                                                                                                    
                                                                                                     A1 = 9.588867f,
                                                                                                     A2=-16.8873f,
                                                                                                     A3 = 36.31816f,
                                                                                                     A4 = 0,
                                                                                                     A5=68.682130f,
                                                                                                     A6 = -0.261475f
                                                                                                    
                                                                                                  }
                                                                      },
                                                                   {
                                                                      Positions.CameraBack, new AxisPosition
                                                                                                  {
                                                                                                     //A1 = 21.63691f,
                                                                                                     //A2=-26.6168f,
                                                                                                     //A3 = 55.77891f,
                                                                                                     //A4 = 0,
                                                                                                     //A5=58.90649f,
                                                                                                     //A6 = -112.4078f
                                                                                                    
                                                                                                     A1 = 9.588867f,
                                                                                                     A2=-16.8873f,
                                                                                                     A3 = 36.31816f,
                                                                                                     A4 = 0,
                                                                                                     A5=68.682130f,
                                                                                                     A6 = 180f-0.261475f
                                                                                                  }
                                                                      },
                                                                };

      CubeModel<CubeSides> Cube;
      public kr3_calc()
      {
         ResetCube();
      }
      public void ResetCube()
      {
         Cube = new CubeModel<CubeSides>(new[]{
             (CubeSides)0,
             (CubeSides)1,
             (CubeSides)2,
             (CubeSides)3,
             (CubeSides)4,
             (CubeSides)5});
      }

      /// <summary>
      /// Evaluiert die aktuelle Bewegung, indem es in dem Würfelmodell die zu drehende Seite sucht
      /// und Diese anschließend durch die zur verfügung stehenden Bewegungen nach unten kippt.
      /// 
      /// </summary>
      /// <param name="Solution"></param>
      public List<KR3Moves> EvalutateSolution(List<CubeMoves> Solution)
      {
         List<KR3Moves> Kr3Moves = new List<KR3Moves>(300);
         for (int i = 0; i < Solution.Count; i++)
         {
            CubeSides SideToRotate = (CubeSides)((uint)Solution[i] >> 16);
            CubeSides CurrentSide = Cube.FindSide(SideToRotate);
            CubeRotation CubeRotation = (CubeRotation)((uint)Solution[i] & 0x0000ffff);
            switch (CurrentSide)
            {
               case CubeSides.Back:
                  RotateBack(ref Kr3Moves, CubeRotation);
                  break;
               case CubeSides.Top:
                  RotateTop(ref Kr3Moves, CubeRotation);
                  break;
               case CubeSides.Bottom:
                  RotateBottom(ref Kr3Moves, CubeRotation);
                  break;
               case CubeSides.Left:
                  RotateLeft(ref Kr3Moves, CubeRotation);
                  break;
               case CubeSides.Right:
                  RotateRight(ref Kr3Moves, CubeRotation);
                  break;
               case CubeSides.Front:
                  RotateFront(ref Kr3Moves, CubeRotation);
                  break;
            }
         }
         return Kr3Moves;
      }
      void RotateFront(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //2 mal nach unten kippen
         Kr3Moves.Add(KR3Moves.FlipToFront);
         Cube.TurnDown();
         Kr3Moves.Add(KR3Moves.FlipToFront);
         Cube.TurnDown();
         Rotate(ref Kr3Moves, Rotation);
      }
      void RotateLeft(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //Erst nach links drehen, dann dach unten kippen
         Kr3Moves.Add(KR3Moves.MoveLeft);
         Cube.RotateLeft();
         Kr3Moves.Add(KR3Moves.FlipToFront);
         Cube.TurnDown();
         Rotate(ref Kr3Moves, Rotation);
      }
      void RotateRight(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //Erst nach rechts drehen, dann nach unten kippen
         Kr3Moves.Add(KR3Moves.MoveRight);
         Cube.RotateRight();
         Kr3Moves.Add(KR3Moves.FlipToFront);
         Cube.TurnDown();
         Rotate(ref Kr3Moves, Rotation);
      }
      void RotateBack(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //No Need to move, just twist
         Rotate(ref Kr3Moves, Rotation);
      }
      void RotateTop(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //nach oben kippen
         Kr3Moves.Add(KR3Moves.FlipToBack);
         Cube.TurnUp();
         Rotate(ref Kr3Moves, Rotation);
      }
      void RotateBottom(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         //Nach unten kippen
         Kr3Moves.Add(KR3Moves.FlipToFront);
         Cube.TurnDown();
         Rotate(ref Kr3Moves, Rotation);
      }
      void Rotate(ref List<KR3Moves> Kr3Moves, CubeRotation Rotation)
      {
         bool Rotated = false;
         if (Kr3Moves.Count > 0)
         {
            if (Kr3Moves[Kr3Moves.Count - 1] == KR3Moves.RotateLeft && Rotation == CubeRotation.Left)
            {
               Kr3Moves[Kr3Moves.Count - 1] = KR3Moves.Rotate180;
               Rotated = true;
            }

            if (Kr3Moves[Kr3Moves.Count - 1] == KR3Moves.RotateRight && Rotation == CubeRotation.Right)
            {
               Kr3Moves[Kr3Moves.Count - 1] = KR3Moves.Rotate180;
               Rotated = true;
            }
         }
         if (!Rotated)
         {
            KR3Moves krRotation = (Rotation == CubeRotation.Left) ? KR3Moves.RotateLeft : KR3Moves.RotateRight;
            Kr3Moves.Add(krRotation);
         }
         if (Rotation == CubeRotation.Left)
            Cube.RotateLeft();
         else if (Rotation == CubeRotation.Right)
            Cube.RotateRight();
      }

      private delegate void KR3MoveDeletage(ICommands commandProcessor);

      private static Dictionary<KR3Moves, KR3MoveDeletage> MoveDelegates = new Dictionary<KR3Moves, KR3MoveDeletage>
                                                                              {
                                                                                 {KR3Moves.MoveLeft, MoveLeft},
                                                                                 {KR3Moves.MoveRight, MoveRight},
                                                                                 {KR3Moves.Move180, Move180},
                                                                                 {KR3Moves.FlipToBack, FlipToBack},
                                                                                 {KR3Moves.FlipToFront, FlipToFront},
                                                                                 {KR3Moves.RotateLeft, RotateLeft},
                                                                                 {KR3Moves.RotateRight, RotateRight},
                                                                                 {KR3Moves.Rotate180, Rotate180},
                                                                              };



      public static void ExecuteSolution(List<KR3Moves> Solution, ICommands commandProcessor)
      {
         Solution.ForEach(move => MoveDelegates[move](commandProcessor));
      }
      public static void FlipToFront(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FlipTopUp]);
         commander.PTP(AxisPositions[Positions.FlipTopUpToDown]);
         commander.PTP(AxisPositions[Positions.FlipTopDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FlipTopP1]);
         commander.PTP(AxisPositions[Positions.FlipTopP2]);
         commander.PTP(AxisPositions[Positions.FlipFrontUp]);
         commander.PTP(AxisPositions[Positions.FlipFrontUpToDown]);
         commander.PTP(AxisPositions[Positions.FlipFrontDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.FlipFrontUp]);
         commander.PTP(AxisPositions[Positions.FlipTopP2]);
         commander.PTP(AxisPositions[Positions.FlipTopP1]);
      }
      public static void FlipToBack(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FlipTopP1]);
         commander.PTP(AxisPositions[Positions.FlipTopP2]);
         commander.PTP(AxisPositions[Positions.FlipFrontUp]);
         commander.PTP(AxisPositions[Positions.FlipFrontDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FlipFrontUpToDown]);
         commander.PTP(AxisPositions[Positions.FlipFrontUp]);
         commander.PTP(AxisPositions[Positions.FlipTopP2]);
         commander.PTP(AxisPositions[Positions.FlipTopP1]);
         //commander.PTP(AxisPositions[Positions.FlipTopUp]);
         commander.PTP(AxisPositions[Positions.FlipTopUpToDown]);
         commander.PTP(AxisPositions[Positions.FlipTopDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.FlipTopUp]);
      }
      public static void MoveLeft(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
         commander.PTP(AxisPositions[Positions.StartPositionDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
      }
      public static void MoveRight(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
         commander.PTP(AxisPositions[Positions.StartPositionDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
      }
      public static void Move180(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontClockwiseDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
      }


      //Twist(...) Funktionen drehen den würfel.
      //Dabei gilt: 
      // - Der Würfel bleibt in der Halterung, somit wird der würfel verändert
      // - Die angegebene Drehrichtung bezieht sich auf den Greifer. Die sich am Boden befindende seite wird somit entgegengesetzt gedreht.

      public static void RotateLeft(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.StartPositionDownSwingLeft]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);

      }
      public static void RotateRight(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.StartPositionDownSwingRight]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
      }
      public static void Rotate180(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseUp]);
         commander.PTP(AxisPositions[Positions.FrontCounterClockwiseDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.FrontClockwiseDown]);
         //commander.PTP(AxisPositions[Positions.FrontClockwiseDownSwing]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.FrontClockwiseUp]);
      }
      public static void MoveToCameraFront(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
         commander.PTP(AxisPositions[Positions.StartPositionDown]);
         commander.GRAB(true);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
         commander.PTP(AxisPositions[Positions.CameraFront]);
      }
      public static void MoveToCameraFrontToBack(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.CameraBack]);
      }
      public static void MoveFromCameraBackToStart(ICommands commander)
      {
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
         commander.PTP(AxisPositions[Positions.StartPositionDown]);
         commander.GRAB(false);
         commander.PTP(AxisPositions[Positions.StartPositionUp]);
      }
   }
}
